#include "overlap.h"

namespace Nesting
{
	
	/////////////////////////////////////////////////////////////////////////////////////////
	//										Overlap_2									   //
	////////////////////////////////////////////////////////////////////////////////////////

	void Overlap_2::make_keypoints(const Signed_edge& mv_edge, const Signed_edge& st_edge, const Vector_2& v, std::vector<Keypoint>& keypoints)
	{
		using CGAL::squared_distance;
		using CGAL::intersection;
		using CGAL::object_cast;

		const Edge_const_iterator mve = mv_edge.first;
		const Edge_const_iterator ste = st_edge.first;
		Ray_2 mv_src_ray(mve->source(), v), mv_tgt_ray(mve->target(), v);
		Ray_2 st_src_ray(ste->source(), -v), st_tgt_ray(ste->target(), -v);
		CGAL::Object res = intersection(mv_src_ray, *ste);
		int nb_keypoints = 0;
		if (const Point_2 *p = object_cast<Point_2>(&res))
		{
			double d = CGAL::sqrt(squared_distance(*p, mve->source()));
			keypoints.push_back(Keypoint(d, mve, ste, keypoint_type(mv_edge, st_edge)));
			nb_keypoints++;
			keypoints.back().pos = *p;
		}
		res = intersection(mv_tgt_ray, *ste);
		if (const Point_2 *p = object_cast<Point_2>(&res))
		{
			double d = CGAL::sqrt(squared_distance(*p, mve->target()));
			keypoints.push_back(Keypoint(d, mve, ste, keypoint_type(mv_edge, st_edge)));
			nb_keypoints++;
			keypoints.back().pos = *p;
		}
		res = intersection(st_src_ray, *mve);
		if (const Point_2 *p = object_cast<Point_2>(&res))
		{
			double d = CGAL::sqrt(squared_distance(*p, ste->source()));
			keypoints.push_back(Keypoint(d, mve, ste, keypoint_type(mv_edge, st_edge)));
			nb_keypoints++;
			keypoints.back().pos = ste->source();
		}
		res = intersection(st_tgt_ray, *mve);
		if (const Point_2 *p = object_cast<Point_2>(&res) )
		{
			double d = CGAL::sqrt(squared_distance(*p, ste->target()));
			keypoints.push_back(Keypoint(d, mve, ste, keypoint_type(mv_edge, st_edge)));
			nb_keypoints++;
			keypoints.back().pos = ste->target();
		}
		if (nb_keypoints == 0) // no intersection
			return;
		if (nb_keypoints == 1) // only one intersection, may be caused by numeric error
		{
			keypoints.pop_back();
			return;
		}
		if (nb_keypoints == 3)
		{// eliminate duplicates
			unsigned int sz = keypoints.size();
			double d0 = squared_distance(keypoints[sz-3].pos, keypoints[sz-2].pos);
			double d1 = squared_distance(keypoints[sz-2].pos, keypoints[sz-1].pos);
			double d2 = squared_distance(keypoints[sz-3].pos, keypoints[sz-1].pos);
			if (d0 < d1 && d0 < d2)
				keypoints[sz-3] = keypoints[sz-1];
			keypoints.pop_back();
		}
		if (nb_keypoints == 4) // two edges are the same after translation coincidentally
		{
			keypoints.pop_back();
			keypoints.pop_back();
		}
		// calculate function for each key point
		Keypoint *pkp, *skp; // primary and secondary keypoints, respectively
		if (keypoints[keypoints.size() - 1].t <= keypoints[keypoints.size() - 2].t)
		{
			pkp = &keypoints[keypoints.size() - 1];
			skp = &keypoints[keypoints.size() - 2];
		}
		else
		{
			pkp = &keypoints[keypoints.size() - 2];
			skp = &keypoints[keypoints.size() - 1];
		}
		// height of triangle or echelon
		Line_2 bline(pkp->pos, v);
		double h = CGAL::sqrt(squared_distance(skp->pos, bline));
		double c = (skp->t - pkp->t)*CGAL::sqrt(v.squared_length());
		if (std::fabs(c) <= 1e-8) // two edges are parallel to each other
		{
			pkp->coeff.a = skp->coeff.a = 0.0;
			pkp->coeff.b = skp->coeff.b = h;
			//pkp->coeff.c = skp->coeff.c = -h*skp->t + h*c/2;
			pkp->coeff.c = skp->coeff.c = -h*skp->t;
			pkp->t = skp->t;
		}
		else
		{
			pkp->coeff.a = h/(2*c), pkp->coeff.b = -h/c*pkp->t, pkp->coeff.c = (h/(2*c))*pkp->t*pkp->t;
			skp->coeff.a = 0.0, skp->coeff.b = h, skp->coeff.c = -h*skp->t + h*c/2;
			assert(std::fabs(pkp->coeff.a) < 1.0e10);
			assert( boost::math::isfinite(pkp->coeff.a) );		
		}
		
	}

	Overlap_2::Overlap_2(const Nesting_polygon *move_polygon, const Nesting_polygon *static_polygon, const Vector_2& trans_field, double penalty) 
	{
		p = penalty;
		std::vector<Signed_edge> mv_edges, st_edges;
		mv_edges.reserve(move_polygon->size());
		st_edges.reserve(static_polygon->size());
		double threshold = 0.0;
		for (Edge_const_iterator eit = move_polygon->edges_begin(); eit != move_polygon->edges_end(); ++eit)
		{
			// determine whether positive, negative or neutral
			Vector_2 dir = eit->to_vector();
			dir = dir / CGAL::sqrt(dir.squared_length());
			double cp = trans_field.x()*dir.y() - trans_field.y()*dir.x();
			if (cp < -threshold)
				mv_edges.push_back(Signed_edge(eit, POSITIVE));
			else if (cp > threshold)
				mv_edges.push_back(Signed_edge(eit, NEGATIVE));
			else
				mv_edges.push_back(Signed_edge(eit, NEUTRAL));
		}
		for (Edge_const_iterator eit = static_polygon->edges_begin(); eit != static_polygon->edges_end(); ++eit)
		{
			Vector_2 dir = eit->to_vector();
			dir = dir / CGAL::sqrt(dir.squared_length());
			double cp = trans_field.x()*dir.y() - trans_field.y()*dir.x();
			if (cp < -threshold)
				st_edges.push_back(Signed_edge(eit, POSITIVE));
			else if (cp > threshold)
				st_edges.push_back(Signed_edge(eit, NEGATIVE));
			else
				st_edges.push_back(Signed_edge(eit, NEUTRAL));
		}

		// create and classify key points
		std::vector<Keypoint> keypoints;
		keypoints.reserve(2*mv_edges.size()*st_edges.size()); // 2*M*N break points at most
		for (unsigned int i = 0; i < mv_edges.size(); i++)
		{
			if (mv_edges[i].second == NEUTRAL)	continue;
			for (unsigned int j = 0; j < st_edges.size(); j++)
			{
				if (st_edges[j].second == NEUTRAL) continue;
				make_keypoints(mv_edges[i], st_edges[j], trans_field, keypoints);
			}
		}
		std::vector<Keypoint>::iterator it = std::remove_if(keypoints.begin(), keypoints.end(), IsEmpytKeypoint());
		keypoints.resize(it - keypoints.begin());
		if (keypoints.empty())
			return;
		typedef std::map<std::pair<Edge_const_iterator, Edge_const_iterator>, std::vector<Keypoint>::iterator> Edgepair_keypoint;
		// to memorize which breakpoint we have considered
		Edgepair_keypoint edgepair_keypoint; 
		std::sort(keypoints.begin(), keypoints.end(), Keypoint_cmp());
		Quad_piecewise_func::Coeff composite_coeff(0.0, 0.0, 0.0);
		overlap_area.begin_insert();
		for (unsigned int i = 0; i < keypoints.size()  /*&& keypoints[i].t <= max_trans*/; i++)
		{
			std::pair<Edge_const_iterator, Edge_const_iterator> edge_pair = std::make_pair(keypoints[i].mv_e, keypoints[i].st_e);
			Edgepair_keypoint::iterator it = edgepair_keypoint.find(edge_pair);
			if (it == edgepair_keypoint.end())
			{// primary key point, edges start to form area
				edgepair_keypoint[edge_pair] = keypoints.begin() + i;
				switch (keypoints[i].type) 
				{
				case Keypoint::POSITIVE_NEGATIVE:
				case Keypoint::NEGATIVE_POSITIVE:
					composite_coeff += keypoints[i].coeff;
					break;
				case Keypoint::POSITIVE_POSITIVE:
				case Keypoint::NEGATIVE_NEGATIVE:
					composite_coeff -= keypoints[i].coeff;
					break;
				}
				
			}
			else
			{// secondary key point
				switch (keypoints[i].type)
				{
				case Keypoint::POSITIVE_NEGATIVE:
				case Keypoint::NEGATIVE_POSITIVE:
					composite_coeff -= it->second->coeff;
					composite_coeff += keypoints[i].coeff;
					break;
				case Keypoint::POSITIVE_POSITIVE:
				case Keypoint::NEGATIVE_NEGATIVE:
					composite_coeff += it->second->coeff;
					composite_coeff -= keypoints[i].coeff;
					break;
				}
			}
			overlap_area.insert_piece(keypoints[i].t, composite_coeff);
		}
		overlap_area.end_insert();
		// whether to append the penalty?
		//double threshold = 1.0e-8;
		unsigned int i;
		for (i = 0; i < keypoints.size(); i++)
		{
			if (std::fabs(keypoints[i].coeff.a) > 1.0e-8 
				|| std::fabs(keypoints[i].coeff.b) > 1.0e-8 
				|| std::fabs(keypoints[i].coeff.c) > 1.0e-8) // if intersect happens
			{
				//Quad_piecewise_func penalty_term;
				//penalty_term.begin_insert();
				//penalty_term.insert_piece(keypoints.front().t, 0.0, 0.0, penalty);
				//penalty_term.insert_piece(keypoints.back().t, 0.0, 0.0, 0.0);
				//penalty_term.end_insert();
				//add(overlap_area, penalty_term, extended_area);
				break;
			}
		}
		if (i == keypoints.size())
			extended_area = overlap_area;
		else
		{				
			Quad_piecewise_func penalty_term;
			penalty_term.begin_insert();
			penalty_term.insert_piece(keypoints.front().t, 0.0, 0.0, penalty);
			penalty_term.insert_piece(keypoints.back().t, 0.0, 0.0, 0.0);
			penalty_term.end_insert();
			add(overlap_area, penalty_term, extended_area);
		}
	}

	Overlap& Overlap_2::operator+=(const Overlap& other)
	{
		const Overlap_2& other_2 = dynamic_cast<const Overlap_2&>(other);
		Quad_piecewise_func sum ;
		add(overlap_area, other_2.overlap_area, sum) ;
		overlap_area = sum;

		Quad_piecewise_func ex_sum;
		add(extended_area, other_2.extended_area, ex_sum);
		extended_area = ex_sum;
		return *this;
	}
	double Overlap_2::min(double &argmin) const
	{
		//return overlap_area.min(argmin);
		return extended_area.min(argmin);
	}
	double Overlap_2::min(double mint, double maxt, double& argmin) const
	{
		//return overlap_area.min(mint, maxt, argmin);
		return extended_area.min(mint, maxt, argmin);
	}
}